
#include "init.h"
#include "PAC.h"
#include "adapter_C_CPP.h"

void system_init(void)
{
  xuint16 delay;
  //PLL settings
  PLLCTL_ACQ = 1;
  PLLCTL_PLLON = 1;
  SYNR = 0x02;
  REFDV = 0x01;
  //wait for a given time
  for (delay=0;delay<0xffff;++delay){} 
  CLKSEL_PLLSEL = 1;
  //EEPROM base
  INITEE = 0x21;
  //AD
  AD_init();
  //SCI
  
  //PACA
  init_PACA();
  set_PACA(0);
  //motor
  motor_init(); 
  timer_init();
  //Timer's settings
  g_timer_set_elapse(3000); //1ms
  DDRT_DDRT2=0;
  DDRT_DDRT3=0;
  //Sensor IO direction
  DDRA=0x00;
  DDRB=0x00;
  //
  DDRH=0xFF;
  
}

void motor_init(void)
{
//PA5~PA7
  //DDRA = (DDRA&0b00011111)|0b11100000;
  DDRK = 0xff;
  
  //PP0(L) & PP3(R)(PWM))
  PWME_PWME0 = 1; //Enable
  PWME_PWME1 = 1;
  
  PWMPOL_PPOL0 = 1; //Polarity
  PWMPOL_PPOL1 = 1;
  //select Clock SB
  PWMCLK_PCLK0 = 1;
  PWMCLK_PCLK1 = 1;

  PWMPRCLK = (PWMPRCLK&0x8F)|0x10; //Clock B = bus / 2  <bus = 24MHz>
  PWMCTL_CON01 = 0; //DONOT concatenat channel 0 and 1
  PWMCTL_CON23 = 0;
  PWMSCLB = 0x30; //Clock SB = Clock B / (2 * PWMSCLB) //
  
  PWMPER0 = 0xff;  //	 //f = 1495Hz
  PWMPER1 = 0xff;
  
  PWMDTY0 = 0x00;  //	 //duty = 
  PWMDTY1 = 0x00;
}

void timer_init(void)
{
  MCCTL = 0xC2; 		 			//MCZI MODMC RDMCL ICLAT FLMC MCEN MCPR1 MCPR0  11000010B		 
                          //Prescaler=8, Clock source=bus clock 
  MCCNT = 3000;					  //1 ms every interrupt
  MCFLG = 0x80;
}

void AD_init(void)
{
  ATD0CTL2=0xC0;	 
	ATD0CTL3=0x4C;
	ATD0CTL4=0x86;
	ATD0CTL5=0xD0;
}